// -*- C++ -*-
/*!
 * @file  LocalizeCenter.cpp
 * @brief ${rtcParam.description}
 * @date $Date$
 *
 * $Id$
 */

#include "LocalizeCenter.h"

// Module specification
// <rtc-template block="module_spec">
static const char* localizecenter_spec[] =
  {
    "implementation_id", "LocalizeCenter",
    "type_name",         "LocalizeCenter",
    "description",       "${rtcParam.description}",
    "version",           "1.0.0",
    "vendor",            "AIST",
    "category",          "example",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "compile",
    "conf.default.startX", "3.4",
    "conf.default.startY", "7.7",
    "conf.default.startTheta", "-1.571",
	"conf.default.cycle", "20",
	"conf.default.odoWeight", "0.0",
	"conf.default.ceilWeight", "1.0",
    ""
  };
// </rtc-template>

/*!
 * @brief constructor
 * @param manager Maneger Object
 */
LocalizeCenter::LocalizeCenter(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_odmIn("dp_odmIn", m_odm),
    m_ceilIn("dp_ceilIn", m_ceil),
    m_dp_out0Out("dp_out0", m_dp_out0)

    // </rtc-template>
{
}

/*!
 * @brief destructor
 */
LocalizeCenter::~LocalizeCenter()
{
}



RTC::ReturnCode_t LocalizeCenter::onInitialize()
{
    bindParameter("startX", m_start_x, "3.4");
    bindParameter("startY", m_start_y, "7.70");
    bindParameter("startTheta", m_start_theta, "-1.571");
    bindParameter("cycle", m_cycle, "20");	
    bindParameter("odoWeight", m_odmWgt, "0.0");
    bindParameter("ceilWeight", m_ceilWgt, "1.0");

	start_x_prev = m_start_x;
	start_y_prev = m_start_y;
	start_theta_prev = m_start_theta;
	
	pos_init_cnt = 0;

    // Registration: InPort/OutPort/Service
    // <rtc-template block="registration">
    // Set InPort buffers

    addInPort("dp_odmIn", m_odmIn);
    addInPort("dp_ceilIn", m_ceilIn);
  
    // Set OutPort buffer
    addOutPort("dp_out0", m_dp_out0Out);
  

	m_odm.data.position.x = m_ceil.data.position.x = m_avPos.x = m_start_x;
	m_odm.data.position.y = m_ceil.data.position.y = m_avPos.y = m_start_y;
	m_odm.data.heading = m_ceil.data.heading = m_avPos.theta = m_start_theta;

	m_totalWgt = m_odmWgt + m_ceilWgt;
	counter = 0;

  return RTC::RTC_OK;
}


RTC::ReturnCode_t LocalizeCenter::onActivated(RTC::UniqueId ec_id)
{
	if( ! ( start_x_prev == m_start_x && start_y_prev == m_start_y && start_theta_prev == m_start_theta ) )
	{
		pos_init_cnt = 0;
		start_x_prev = m_start_x;
		start_y_prev = m_start_y;
		start_theta_prev = m_start_theta;
		
		m_odm.data.position.x = m_ceil.data.position.x = m_avPos.x = m_start_x;
		m_odm.data.position.y = m_ceil.data.position.y = m_avPos.y = m_start_y;
		m_odm.data.heading = m_ceil.data.heading = m_avPos.theta = m_start_theta;

	}
	
	m_dp_out0.data.position.x = m_avPos.x;
	m_dp_out0.data.position.y = m_avPos.y;
	m_dp_out0.data.heading = m_avPos.theta;
	m_dp_out0.tm.sec = m_dp_out0.tm.nsec = 0;

	m_dp_out0Out.write();

  return RTC::RTC_OK;
}


RTC::ReturnCode_t LocalizeCenter::onDeactivated(RTC::UniqueId ec_id)
{
	while(!m_odmIn.isEmpty()){
		m_odmIn.read();
	}
	while(!m_ceilIn.isEmpty()){
		m_ceilIn.read();

	}

	return RTC::RTC_OK;
}

RTC::ReturnCode_t LocalizeCenter::onExecute(RTC::UniqueId ec_id)
{
	int odmFlag=0, ceilFlag=0;

	if(pos_init_cnt < 20){ // First of all, 20 times to output initial pos data.
	  m_dp_out0Out.write();
	  pos_init_cnt ++;
	}else{
	  // If odometry data come, we select the last data which stored in the inport buffer.
	  if(m_odmIn.isNew()){
	    while(!m_odmIn.isEmpty())
	    {
		  m_odmIn.read();
		  //std::cout << "[ODOMET]" << m_odm.tm.sec << "." << m_odm.tm.nsec << "(" << m_odm.data.position.x << ", " << m_odm.data.position.y  << ", " << m_odm.data.heading << ")" << std::endl;
		  odmFlag = 1;
		}
	  }

	  /// localization data come
	  if(m_ceilIn.isNew()){
		// we chose the last localization data.
	    while(!m_ceilIn.isEmpty()){
		  m_ceilIn.read();
		  ceilFlag = 1;
		}

		m_avPos.x = ( m_odmWgt * m_odm.data.position.x + m_ceilWgt * m_ceil.data.position.x ) / m_totalWgt;
		m_avPos.y = ( m_odmWgt * m_odm.data.position.y + m_ceilWgt * m_ceil.data.position.y ) / m_totalWgt;

/*
		//if( m_odm.theta * m_ceil.theta < 0 && abs( m_ceil.theta ) > 3.0 * M_PI / 4.0)
		if( m_odm.data.heading * m_ceil.data.heading < 0 && abs( m_ceil.data.heading ) > 3.0 * M_PI / 4.0)
		{ //π側と-π側で打ち消してしまいそうな時
		  if( m_odm.data.heading > m_ceil.data.heading){
			m_ceil.data.heading = m_ceil.data.heading + 2 * M_PI;
		  }else{
			m_odm.data.heading = m_odm.data.heading + 2 * M_PI;
		  }
*/
		m_avPos.theta = ( m_odmWgt * m_odm.data.heading + m_ceilWgt * m_ceil.data.heading ) / m_totalWgt;

		if (m_avPos.theta > M_PI){
			m_avPos.theta -= -M_PI2;
		}else if (m_avPos.theta < -M_PI){
			m_avPos.theta += M_PI2;
		}

		m_dp_out0.data.position.x = m_avPos.x;
		m_dp_out0.data.position.y = m_avPos.y;
		m_dp_out0.data.heading = m_avPos.theta;
		m_dp_out0.tm.sec = m_ceil.tm.sec;
		m_dp_out0.tm.nsec = m_ceil.tm.nsec;

	    m_dp_out0Out.write();
		//printf("%d %d : X=%f Y=%f Th=%f\n",odmFlag,ceilFlag,m_avPos.x,m_avPos.y,m_avPos.theta);
		counter = 0;

	  }else{
		if(odmFlag == 1){
			m_avPos.x = m_odm.data.position.x;
			m_avPos.y = m_odm.data.position.y;
			m_avPos.theta = m_odm.data.heading;
			

			if (m_avPos.theta > M_PI){
			  m_avPos.theta = -M_PI * 2 + m_avPos.theta;
			}else if (m_avPos.theta < -M_PI){
			  m_avPos.theta = M_PI * 2 + m_avPos.theta;
			}

			m_dp_out0.data.position.x = m_avPos.x;
			m_dp_out0.data.position.y = m_avPos.y;
			m_dp_out0.data.heading = m_avPos.theta;
			m_dp_out0.tm.sec = m_odm.tm.sec;
			m_dp_out0.tm.nsec = m_odm.tm.nsec;

			counter++;

			if(counter % m_cycle == 0){
			  counter = 0;
			  m_dp_out0Out.write();
			  //printf("ODO: %d %d : X=%f Y=%f Th=%f\n", odmFlag,ceilFlag,m_avPos.x,m_avPos.y,m_avPos.theta);
			}
		  }
		}
	}


  return RTC::RTC_OK;
}

extern "C"
{
 
  void LocalizeCenterInit(RTC::Manager* manager)
  {
    coil::Properties profile(localizecenter_spec);
    manager->registerFactory(profile,
                             RTC::Create<LocalizeCenter>,
                             RTC::Delete<LocalizeCenter>);
  }
  
};


